//
// Created by hyj on 17-10-16.
//

// This file is part of SVO - Semi-direct Visual Odometry.
//
// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
// (Robotics and Perception Group, University of Zurich, Switzerland).
//
// SVO is free software: you can redistribute it and/or modify it under the
// terms of the GNU General Public License as published by the Free Software
// Foundation, either version 3 of the License, or any later version.
//
// SVO is distributed in the hope that it will be useful, but WITHOUT ANY
// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program.  If not, see <http://www.gnu.org/licenses/>.

#include <svo/config.h>
#include <svo/frame_handler_mono.h>
#include <svo/map.h>
#include <svo/frame.h>
#include <vector>
#include <string>
#include <svo/math_lib.h>
#include <svo/camera_model.h>
#include <opencv2/opencv.hpp>
#include <sophus/se3.h>
#include <iostream>

#include <svo/slamviewer.h>
#include<thread>


namespace svo {

void LoadImages(const std::string &strFile, std::vector<std::string> &vstrImageFilenames, std::vector<double> &vTimestamps)
{
    std::ifstream f;
    f.open(strFile.c_str());

    // skip first three lines
    std::string s0;
    std::getline(f,s0);
    std::getline(f,s0);
    std::getline(f,s0);

    while(!f.eof())
    {
        std::string s;
        std::getline(f,s);
        if(!s.empty())
        {
            std::stringstream ss;
            ss << s;

            std::stringstream sRGB;
            sRGB<< s << ".png";

            double t;
            ss >> t;
            vTimestamps.push_back(t);
            //std::cout<<sRGB.c_str()<<std::endl;
            vstrImageFilenames.push_back(sRGB.str());
        }
    }
}

class BenchmarkNode
{
    svo::AbstractCamera* cam_;
    svo::PinholeCamera* cam_pinhole_;
    svo::FrameHandlerMono* vo_;

    SLAM_VIEWER::Viewer* viewer_;
    std::thread * viewer_thread_;

public:
    BenchmarkNode();
    ~BenchmarkNode();
    void runFromFolder();
};

BenchmarkNode::BenchmarkNode()
{

    cam_ = new svo::PinholeCamera(752,480,718.8629706976196,717.8381934509487, 370.04788336179877, 223.33284123255405,
                                  0.07736574173292003, -0.09856818578064286, -0.0023534432819480988, 0.0003080404768945955);

    vo_ = new svo::FrameHandlerMono(cam_);
    vo_->start();

    viewer_ = new SLAM_VIEWER::Viewer(vo_);
    viewer_thread_ = new std::thread(&SLAM_VIEWER::Viewer::run,viewer_);
    viewer_thread_->detach();

}

BenchmarkNode::~BenchmarkNode()
{
    delete vo_;
    delete cam_;
    delete cam_pinhole_;

    delete viewer_;
    delete viewer_thread_;
}


void BenchmarkNode::runFromFolder()
{
    std::vector<std::string> vstrImageFilenames;
    std::vector<double> vTimestamps;
    std::string filepath = std::string("/media/hyj/DISK_IMG/vo/cam2");
    std::string strFile = "/media/hyj/DISK_IMG/vo/img_time_stamp.txt";
    LoadImages(strFile, vstrImageFilenames, vTimestamps);

    int nImages = vstrImageFilenames.size();
    cv::Mat img;
    std::cout<< "start pub datasets img"<<std::endl;
    std::ofstream hyj_file;
    hyj_file.open("campose_readface.txt");
    for(int ni=50; ni<nImages; ni++)
    {
        std::string img_path = filepath+"/"+vstrImageFilenames[ni];
        img = cv::imread(img_path.c_str(),CV_LOAD_IMAGE_GRAYSCALE);
        assert(!img.empty());

        // process frame
        vo_->addImage(img, vTimestamps[ni]);

        // display tracking quality
        Eigen::Matrix3d Rrc;
        Rrc<< -0.991575,  0.127984, 0.018307,
            -0.127481, -0.991533, 0.0270592,
            0.0215967, 0.0245139,  0.999495;
        Sophus::SE3 Trc_ = Sophus::SE3 (Rrc, Eigen::Vector3d(0.0323693,0.08,0.0));

        //if(vo_->lastFrame() != NULL  && vo_->lastFrame()->is_keyframe_)
        if(vo_->lastFrame() != NULL)
        {

            std::cout << "Frame-Id: " << vo_->lastFrame()->id_ << " \t"
                      << "#Features: " << vo_->lastNumObservations() << " \n";

            //std::cout<<"Frame pose: "<< vo_->lastFrame()->T_f_w_ <<std::endl;
            if(hyj_file.is_open())
            {
                
		//保存相机姿态, 用于标定
                Sophus::SE3 T_world_cam(vo_->lastFrame()->T_f_w_.inverse());
                //Sophus::SE3 T_world_cam( Trc_ * vo_->lastFrame()->T_f_w_.inverse() * Trc_.inverse());
                
                Eigen::Quaterniond q(T_world_cam.rotation_matrix());
                hyj_file<<std::fixed<< vTimestamps[ni] << " "
                        << T_world_cam.translation()(0) << " "
                        << T_world_cam.translation()(1) << " "
                        << T_world_cam.translation()(2) << " "
                        << q.x() << " "
                        << q.y() << " "
                        << q.z() << " "
                        << q.w() << " "
                        << std::endl;

            }

        }
    }
    hyj_file.close();

}

} // namespace svo

int main(int argc, char** argv)
{

    svo::BenchmarkNode benchmark;
    benchmark.runFromFolder();

    printf("BenchmarkNode finished.\n");
    return 0;
}

